#include "DetectionAlgorithm.h"

namespace varayoc
{
   namespace vigilance
   {
      namespace util
      {
         DetectionAlgorithm::DetectionAlgorithm()
         {
            MNToMts  = mc.fromMilesToMeters(1); // 1852
         }

         Bool  DetectionAlgorithm::verifyTrackInsideRingSector(Double   centerLatitude,
                                                               Double   centerLongitude,
                                                               Float    initialAngle,
                                                               Float    spanAngle,
                                                               Float    minRadio,
                                                               Float    maxRadio,
                                                               Double   currentLatitude,
                                                               Double   currentLongitude
                                                               )
         {
            m_Center       = GeoCoordinate_ptr(new GeoCoordinate());
            m_Track        = GeoCoordinate_ptr(new GeoCoordinate());

            m_Center->setLatLon(centerLatitude, centerLongitude);
            m_Track->setLatLon(currentLatitude, currentLongitude);

            BearingDistanceResult   bd;
            bd = m_Center->calculateBearingDistance(m_Track);
            Double distance = mc.fromMilesToMeters(bd.distance);

            // reset
            m_Center.reset();
            m_Track.reset();

            Float radioMenor  = 0;
            Float radioMayor  = 0;
            Float angleMenor  = 0;
            Float angleMayor  = 0;

            radioMenor  = minRadio;
            radioMayor  = maxRadio;

            angleMenor  = initialAngle;
            angleMayor  = initialAngle + spanAngle;
///*
            cout  <<    "RADIO MN ["   << radioMenor/MNToMts
                  << " <= "   << distance/MNToMts
                  << " <= "   << radioMayor/MNToMts  << "]\n"
                  <<    "ANGULO   ["   << angleMenor
                  << " <= "   << bd.bearing
                  << " <= "   << angleMayor  << "]"
                  << endl;
//*/
            if (radioMenor <= distance && distance <= radioMayor && angleMenor <= bd.bearing && bd.bearing <= angleMayor)
               return True;

            return False;
         }

         Bool  DetectionAlgorithm::verifyTrackInsidePolygon(
                                             varayoc::Vector<varayoc::Float64>::Type         m_Latitudes,
                                             varayoc::Vector<varayoc::Float64>::Type         m_Longitudes,
                                             Double   currentLatitude,
                                             Double   currentLongitude
                                             )
         {
            UInt sz = m_Latitudes.size();
            const int numPoints = sz;
            if (sz != m_Longitudes.size())   return False;
            for (UInt i = 0; i < sz; i++)
            {
               if ((currentLatitude == m_Latitudes[i]) && (currentLongitude == m_Longitudes[i]) )
               {
                  cout << "vertice........................." << endl;
                  return   True;
               }
            }

            Float Longitudes[numPoints];
            Float Latitudes[numPoints];
            for (UInt i = 0; i < sz; i++)
            {
               Longitudes[i]  = m_Longitudes[i];
               Latitudes[i]   = m_Latitudes[i];
               cout << "Longitudes[i] : " << Longitudes[i] << " - Latitudes[i] : " << Latitudes[i] << endl;
            }

            return (pnpoly(sz, Latitudes, Longitudes, currentLatitude, currentLongitude) == 1);
         }

         newCoordinatesResult DetectionAlgorithm::generateNewBoundaryCoordinates(Float maxLat,
                                                                                 Float maxLon,
                                                                                 Float minLat,
                                                                                 Float minLon,
                                                                                 Float factor)
         {
            newCoordinatesResult result;
            result.newMaxLat  = 0;
            result.newMaxLon  = 0;
            result.newMinLat  = 0;
            result.newMinLon  = 0;

            m_Center       = GeoCoordinate_ptr(new GeoCoordinate());
            m_Track        = GeoCoordinate_ptr(new GeoCoordinate());

            m_Track->setLatLon(maxLat, maxLon);    // PA
            m_Center->setLatLon(minLat, minLon);   // PB

            BearingDistanceResult   bd;
            bd = m_Center->calculateBearingDistance(m_Track);

            m_Track->move(bd.bearing, factor*bd.distance);

            m_Center->move((bd.bearing+180), factor*bd.distance);

            result.newMaxLat  = m_Track->getLatitude();
            result.newMaxLon  = m_Track->getLongitude();
            result.newMinLat  = m_Center->getLatitude();
            result.newMinLon  = m_Center->getLongitude();

            // reset
            m_Center.reset();
            m_Track.reset();
//            cout << "maxLat   = " << maxLat  << " - maxLon  = " << maxLon  << " - minLat  = " << minLat  << " - minLon  = " << minLon  << endl;
//            cout << "fMaxLat  = " << result.newMaxLat  << " - fMaxLon = " << result.newMaxLon  << " - fMinLat = " << result.newMinLat  << " - fMinLon = " << result.newMinLon  << endl;

            return   result;
         }

         Vector<String>::Type   DetectionAlgorithm::calculateUniversalLatLon(Vector<Float64>::Type  allLatitudes,
                                                                              Vector<Float64>::Type  allLongitudes,
                                                                              Float factor
                                                                              )
         {
            Vector<String>::Type   lasLimites;
            UInt sz = allLatitudes.size();
            if (sz != allLongitudes.size())   return lasLimites;

            const int numPoints = sz;
            Float64 Longitudes[numPoints];
            Float64 Latitudes[numPoints];
            for (UInt i = 0; i < sz; i++)
            {
               Latitudes[i]   = allLatitudes[i];
               Longitudes[i]  = allLongitudes[i];
            }
            newCoordinatesResult result;
            result   = generateNewBoundaryCoordinates(*max_element(Latitudes, Latitudes + sz),
                                                      *max_element(Longitudes, Longitudes + sz),
                                                      *min_element(Latitudes, Latitudes + sz),
                                                      *min_element(Longitudes, Longitudes + sz),
                                                      factor
                                                      );
            char cTmp[10];
            sprintf(cTmp, "%f" ,result.newMaxLat);
            lasLimites.push_back(cTmp);   //0
            sprintf(cTmp, "%f" ,result.newMaxLon);
            lasLimites.push_back(cTmp);   //1
            sprintf(cTmp, "%f" ,result.newMinLat);
            lasLimites.push_back(cTmp);   //2
            sprintf(cTmp, "%f" ,result.newMinLon);
            lasLimites.push_back(cTmp);   //3

//            /*
            cout << "UNIVERSO - ";
            cout <<  "Lat["   << lasLimites[2];
            cout <<  ","      << lasLimites[0] << "] - ";
            cout <<  "Lon["   << lasLimites[3];
            cout <<  ","      << lasLimites[1] << "]" << endl;
//            */
            return lasLimites;
         }

         int   DetectionAlgorithm::pnpoly(int npol, float *xp, float *yp, float x, float y)
         {
            int i, j, c = 0;
            for (i = 0, j = npol-1; i < npol; j = i++) {
               if ((((yp[i] <= y) && (y < yp[j])) ||
                    ((yp[j] <= y) && (y < yp[i]))) &&
                   (x < (xp[j] - xp[i]) * (y - yp[i]) / (yp[j] - yp[i]) + xp[i]))
                  c = !c;
            }
            return c;
         }


      }
   }
}
